/*
* @File Name pick_place.c
* @File Path M:\MAS2\PRM_Robotic_Arm\PRMCcode\coding\task\task_pickAndPlace\pick_place.c
* @Author: Ruige_Lee
* @Date:   2019-05-09 13:47:15
* @Last Modified by:   Ruige_Lee
* @Last Modified time: 2019-05-19 16:22:29
* @Email: 295054118@whut.edu.cn
* @page: https://whutddk.github.io/
*/

#include "stdint.h"
#include "route/trajectory/trajectory.h"
#include "iK/iK.h"

void task_pickAndPlace()
{

	// for ( uint8_t robot = 0; robot < 2; robot ++ )
	// {
	// 	float robotConfig[6];
	// 	//get perious place
		
	// 	for (uint8_t axis = 0; axis < 6; axis ++)
	// 	{
	// 		robotConfig[axis] = stepper_getRad( robot, axis );
	// 	}
		

	// 	//move to place 0
	// 	Cspline[robot].count = 6;
	// 	for ( uint8_t p = 0; p < 6; p++ )
	// 	{
	// 		for (uint8_t axis = 0; axis < 6; axis ++)
	// 		{

	// 			Cspline[robot].points[p].position[axis] = robotConfig[axis] + p * ((-robotConfig[axis]) / 5);
	// 		}
	// 		Cspline[robot].points[p].timePoint = 0.4*p;
	// 	}

	// 	tra_checkIn(robot);
	// 	while( FIFO_empty[robot] != 0 )
	// 		{;}
	// 	while( FIFO_empty[robot] != 1 )
	// 	{;}
		
	// }	

	while(1)
	{	
		for ( uint8_t robot = 0; robot < 2; robot ++ )
		{
			float startConfig[6];
			float endConfig[6];
			float step[6];

			//get perious place
			for (uint8_t axis = 0; axis < 6; axis ++)
			{
				startConfig[axis] = stepper_getRad( robot, axis );
			}
			
			endConfig[0] = 0.875;
			endConfig[1] = 0;
			endConfig[2] = 0;
			endConfig[3] = 0;
			endConfig[4] = 0;
			endConfig[5] = 0;

			for ( uint8_t axis = 0; axis < 6; axis++ )
			{
				step[axis] = ( endConfig[axis] - startConfig[axis] ) / 10;
			}


			//move to checkpoint 1
			Cspline[robot].count = 11;
			for ( uint8_t p = 0; p < 11; p++ )
			{

				for ( uint8_t axis = 0; axis < 6; axis++ )
				{
					Cspline[robot].points[p].position[axis] = startConfig[axis] + p * step[axis];
				}

				Cspline[robot].points[p].timePoint = 0.2*p;
			}
			tra_checkIn(robot);
		}


		while( FIFO_empty[0] != 0 )
			{;}
		while( FIFO_empty[0] != 1 )
		{;}
			

	
		float startPose[3];
		float endPose[3];
	
		for ( uint8_t robot = 0; robot < 2; robot ++ )
		{	
			//cal high to low and push to tra


			ik_fkCheck(robot,startPose);


			// startPose[0] = 000;
			// startPose[1] = 225;
			// startPose[2] = 50;

			endPose[0] = startPose[0];
			endPose[1] = startPose[1];
			endPose[2] = 100;

			ik_checkIn(robot, startPose, endPose, 500);
			ik_checkIn(robot, endPose, startPose, 500);

		}



		while( FIFO_empty[0] != 0 )
		{;}
		while( FIFO_empty[0] != 1 )
		{;}



		for ( uint8_t robot = 0; robot < 2; robot ++ )
		{
			float startConfig[6];
			float endConfig[6];
			float step[6];

			//get perious place
			for (uint8_t axis = 0; axis < 6; axis ++)
			{
				startConfig[axis] = stepper_getRad( robot, axis );
			}
			
			endConfig[0] = -0.875;
			endConfig[1] = 0;
			endConfig[2] = 0;
			endConfig[3] = 0;
			endConfig[4] = 0;
			endConfig[5] = 0;

			for ( uint8_t axis = 0; axis < 6; axis++ )
			{
				step[axis] = ( endConfig[axis] - startConfig[axis] ) / 10;
			}


			//move to checkpoint 1
			Cspline[robot].count = 11;
			for ( uint8_t p = 0; p < 11; p++ )
			{

				for ( uint8_t axis = 0; axis < 6; axis++ )
				{
					Cspline[robot].points[p].position[axis] = startConfig[axis] + p * step[axis];
				}

				Cspline[robot].points[p].timePoint = 0.2*p;
			}
			tra_checkIn(robot);

		}

		while( FIFO_empty[0] != 0 )
			{;}
		while( FIFO_empty[0] != 1 )
		{;}
			


		for ( uint8_t robot = 0; robot < 2; robot ++ )
		{	
			//cal high to low and push to tra
			float startPose[3];
			float endPose[3];

			ik_fkCheck(robot,startPose);


			// startPose[0] = 000;
			// startPose[1] = 225;
			// startPose[2] = 50;

			endPose[0] = startPose[0];
			endPose[1] = startPose[1];
			endPose[2] = 100;

			ik_checkIn(robot, startPose, endPose, 500);
			ik_checkIn(robot, endPose, startPose, 500);
		}

		while( FIFO_empty[0] != 0 )
		{;}
		while( FIFO_empty[0] != 1 )
		{;}


	
				// ik_fkCheck(robot,endPose);

	}

}

